//
// Created by bobin on 17-11-15.
//

#ifndef STRUCT_BUNDLE_ADJUSTMENT_G2OTYPES_H
#define STRUCT_BUNDLE_ADJUSTMENT_G2OTYPES_H


#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>

namespace g2o {

    typedef Eigen::Matrix<double, 6, 1, Eigen::ColMajor> Vector6D;
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6> > BlockSolver_6_6;


    class VertexSBALine : public BaseVertex<6, Vector6D> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        VertexSBALine() : BaseVertex<6, Vector6D>() {
        }

        virtual bool read(std::istream &is);

        virtual bool write(std::ostream &os) const;

        virtual void setToOriginImpl() {
            _estimate.fill(0.);
        }

        virtual void oplusImpl(const double *update) {
            Eigen::Map<const Vector6D> v(update);
            _estimate += v;
        }
    };

    class VertexSBAPlane : public BaseVertex<4, Vector4D> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        VertexSBAPlane() : BaseVertex<4, Vector4D>() {}

        virtual bool read(std::istream &is) override { return false; }

        virtual bool write(std::ostream &os) const override { return true; }

        virtual void setToOriginImpl() {
            _estimate.fill(0);
        }

        virtual void oplusImpl(const double *update) {
            Eigen::Map<const Vector4D> v(update);
            _estimate += v;
        }
    };

    class EdgeProjectLine : public BaseBinaryEdge<4, Vector4D, VertexSBALine, VertexSE3Expmap> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

        EdgeProjectLine();

        bool read(std::istream &is);

        bool write(std::ostream &os) const;

        virtual void computeError() override;

        CameraParameters *_cam;
    };

    class EdgeLineSBAPoint : public BaseBinaryEdge<2, Vector2D, VertexSBALine, VertexSBAPointXYZ> {
    public:
        EdgeLineSBAPoint() : BaseBinaryEdge<2, Vector2D, VertexSBALine, VertexSBAPointXYZ>() {};

        bool read(std::istream &is) override { return true; }

        bool write(std::ostream &os) const override { return true; }

        virtual void computeError() override;

        virtual void linearizeOplus() override;
    };


    class VertexPluckerLine : public BaseVertex<6, Vector6D> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        VertexPluckerLine() : BaseVertex<6, Vector6D>() {}

        virtual bool read(std::istream &is);

        virtual bool write(std::ostream &os) const;

        virtual void setToOriginImpl() {
            _estimate.fill(0.);
        }

        virtual void oplusImpl(const double *update) {
            Eigen::Map<const Vector6D> v(update);
            _estimate += v;
        }
    };

    typedef Eigen::Matrix<double, 3, 6> PluckerMatrix;

    class EdgePluckerLine : public BaseBinaryEdge<4, Vector4D, VertexPluckerLine, VertexSE3Expmap> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

        EdgePluckerLine();

        bool read(std::istream &is);

        bool write(std::ostream &os) const;

        virtual void computeError() override;

        virtual void linearizeOplus() override;

        CameraParameters *_cam;
    };

    class EdgePlaneSBAPoint : public BaseBinaryEdge<1, double, VertexSBAPlane, VertexSBAPointXYZ> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

        EdgePlaneSBAPoint() : BaseBinaryEdge<1, double, VertexSBAPlane, VertexSBAPointXYZ>() {}

        virtual bool read(std::istream &is) override { return true; }

        virtual bool write(std::ostream &os) const override { return true; }

        virtual void computeError() override ;

        virtual void linearizeOplus() override ;
    };
}


#endif //STRUCT_BUNDLE_ADJUSTMENT_G2OTYPES_H
